#include "glQuaternion.h"
#include "math_funcs.h"

glQuaternion::glQuaternion() {
	// constructor
    m_x = m_y = m_z = 0.0f;
    m_w = 1.0f;
}

glQuaternion::~glQuaternion() {
	// destructor
}

// create a quaternion that specifies a rotation (local in one axis)
void glQuaternion::CreateFromAxisAngle(GLfloat x, GLfloat y, GLfloat z, GLfloat degrees) {
    // First we want to convert the degrees to radians 
    // since the angle is assumed to be in radians
    GLfloat angle = GLfloat((degrees / 180.0f) * PI);
    // Here we calculate the sin( theta / 2) once for optimization
    GLfloat result = (GLfloat)sin( angle / 2.0f );
		
    // Calcualte the w value by cos( theta / 2 )
    m_w = (GLfloat)cos( angle / 2.0f );
    // Calculate the x, y and z of the quaternion
    m_x = GLfloat(x * result);
    m_y = GLfloat(y * result);
    m_z = GLfloat(z * result);
}

void glQuaternion::CreateMatrix(GLfloat *pMatrix) {
    // Make sure the matrix has allocated memory to store the rotation data
    if(!pMatrix) return;
    // First row
    pMatrix[ 0] = 1.0f - 2.0f * ( m_y * m_y + m_z * m_z ); 
    pMatrix[ 1] = 2.0f * (m_x * m_y + m_z * m_w);
    pMatrix[ 2] = 2.0f * (m_x * m_z - m_y * m_w);
    pMatrix[ 3] = 0.0f;  
    // Second row
    pMatrix[ 4] = 2.0f * ( m_x * m_y - m_z * m_w );  
    pMatrix[ 5] = 1.0f - 2.0f * ( m_x * m_x + m_z * m_z ); 
    pMatrix[ 6] = 2.0f * (m_z * m_y + m_x * m_w );  
    pMatrix[ 7] = 0.0f;  
    // Third row
    pMatrix[ 8] = 2.0f * ( m_x * m_z + m_y * m_w );
    pMatrix[ 9] = 2.0f * ( m_y * m_z - m_x * m_w );
    pMatrix[10] = 1.0f - 2.0f * ( m_x * m_x + m_y * m_y );  
    pMatrix[11] = 0.0f;  
    // Fourth row
    pMatrix[12] = 0;  
    pMatrix[13] = 0;  
    pMatrix[14] = 0;  
    pMatrix[15] = 1.0f;
    // Now pMatrix[] is a 4x4 homogeneous matrix that can be applied to an OpenGL Matrix
}

glQuaternion glQuaternion::operator *(glQuaternion q)
{
    glQuaternion r;

	q.normalize();

    r.m_w = m_w*q.m_w - m_x*q.m_x - m_y*q.m_y - m_z*q.m_z;
    r.m_x = m_w*q.m_x + m_x*q.m_w + m_y*q.m_z - m_z*q.m_y;
    r.m_y = m_w*q.m_y + m_y*q.m_w + m_z*q.m_x - m_x*q.m_z;
    r.m_z = m_w*q.m_z + m_z*q.m_w + m_x*q.m_y - m_y*q.m_x;
	
    return r;
}

// normalizing the quaternion is a good thing since errors in calculations are accumulated
void glQuaternion::normalize() {

	float TOLERANCE = 0.00001;

	float m = m_w*m_w + m_x*m_x + m_y*m_y + m_z*m_z;

	// only normalize if necessary
	if (  m!=0.f && (fabs(m - 1.0f) > TOLERANCE)) {
		float mag = sqrt(m);
		m_w /= mag;
		m_x /= mag;
		m_y /= mag;
		m_z /= mag;
	}
}

void glQuaternion::EulerToQuat(const float x, const float y, const float z) {
	double	ex, ey, ez;		// temp half euler angles
	double	cr, cp, cy, sr, sp, sy, cpcy, spsy;		// temp vars in roll,pitch yaw

	ex = DEG2RAD(x) / 2.0;	// convert to rads and half them
	ey = DEG2RAD(y) / 2.0;
	ez = DEG2RAD(z) / 2.0;

	cr = cos(ex);
	cp = cos(ey);
	cy = cos(ez);

	sr = sin(ex);
	sp = sin(ey);
	sy = sin(ez);

	cpcy = cp * cy;
	spsy = sp * sy;

	this->m_w = float(cr * cpcy + sr * spsy);

	this->m_x = float(sr * cpcy - cr * spsy);
	this->m_y = float(cr * sp * cy + sr * cp * sy);
	this->m_z = float(cr * cp * sy - sr * sp * cy);

	normalize();

	return;
}	// end void EulerToQuat(..)


void glQuaternion::GetAxisAngle(glVector &v, float &angle) const
{
	double	temp_angle;		// temp angle
	double	scale;			// temp vars

	temp_angle = acos(m_w);

	//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	// Another version where scale is sqrt (x2 + y2 + z2)
	//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	scale = (float)sqrt(m_x*m_x + m_y*m_y + m_z*m_z);

//	assert(0 <= temp_angle);		// make sure angle is 0 - PI
//	assert(PI >= temp_angle);

	if (FLOAT_EQ(0.0f, scale))		// angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0
	{
		angle = 0.0f;

		v.SetValues(0.0f, 0.0f, 1.0f);		// any axis will do
	}
	else
	{
		angle = (float)(temp_angle * 2.0);		// angle in radians

		v.SetValues(float(m_x / scale), float(m_y / scale), float(m_z / scale));
		v.Normalize();

//		assert(0.0f <= angle);			// make sure rotation around axis is 0 - 360
//		assert(2*PI >= angle);
//		assert(v.IsUnit());				// make sure a unit axis comes up
	}

	return;
}	// end void GetAxisAngle(..)

#include <iostream>
void glQuaternion::Print()
{
	std::cout << "Quaternion is " << m_x << " " << m_y << " " << m_z << " w is " << m_w << std::endl;
}

